from franka_servo import FrankaPosServo

franka_servo = FrankaPosServo()

init_pos = [0.60,0.10,0.28]
print("Initial position:", init_pos)
cmd = franka_servo.gen_abs_move_cmd(init_pos, 0.03)
franka_servo.blocking_move(cmd)
print("Moved back to initial position:", franka_servo.robot_position)
print("Experiment execution completed.")